#include "stm32f10x.h"
#include <stdlib.h>
#include "sys.h"
#include "delay.h"
#include "OLED.h"
#include "PWM.h"
#include "control.h"
#include "temer.h"
#include "Key.h"

int Moto;
int Target_Velocity=20,Position=750,Target_Position=750;
float Position_kP=120,Position_KI=0.1,Position_KD=500;

uint16_t CPWM[9] = {1500,1500,1500,1500,1500,1500,1500,1500,1500};
uint16_t pos[2][9] = {
	{4500,500,500,500,500,500,500,500,500},
	{4500,2500,2500,2500,2500,2500,2500,2500,2500}
};
double dp;
double dp0[9];
char point_now=0;
char point_aim=1;
uint16_t m=1;
uint16_t n=1000;

uint8_t flag_vpwm=0;
uint8_t flag_pwm;

uint8_t key,num;
void change(void);
void vpwm(void);
int main(void)
{
    SystemInit();
    delay_init();
    NVIC_Configuration();
	PWM_Init();
	TIM4_Configuration();
	TIM3_Configuration();
    OLED_Init();
	Key_Init();
	change();
    while (1) {
		key = Key_GetNum();
		if(key)
		{
			num++;
		}
		if(flag_vpwm==1)
		{
			OLED_ShowNum(1,1,num,4);
			OLED_ShowNum(2,1,Moto,10);
			OLED_ShowNum(3,1,Target_Position,10);
			OLED_ShowNum(4,1,Target_Velocity,10);
//			vpwm();
//			PWM_SetCompare1(CPWM[1]);
//			PWM_SetCompare2(CPWM[2]);
//			Moto=Position_PID(Position,CPWM[2]);
//			Moto=Xianfu(Moto,Target_Velocity);
//			Set_Pwm(Moto);
			flag_vpwm=0;
		}
    }
}

void change(void)
{
	u8 s;
	if(point_aim==1)
	{
		point_aim=0;
		point_now=1;
	}else{
		point_aim=1;
		point_now=0;
	}
	n=pos[point_aim][0]/20;
	for(s=1;s<9;s++)
	{
		if(pos[point_aim][s]>pos[point_now][s])
		{
			dp=pos[point_aim][s]-pos[point_now][s];
			dp0[s]=dp/n;
		}
		if(pos[point_aim][s]<=pos[point_now][s])
		{
			dp=pos[point_now][s]-pos[point_aim][s];
			dp0[s]=dp/n;
			dp0[s]=-dp0[s];
		}
	}
	m=0;
}
void vpwm(void)
{
	u8 j=0;
	static u8 flag_Tover;
	m++;
	if(m==n)
	{
		flag_Tover=1;
	}
	for(j=1;j<9;j++)
	{
		if(abs(CPWM[j]-pos[point_aim][j])<5)
		{
			CPWM[j]=pos[point_aim][j];
		}else{
			CPWM[j]=pos[point_now][j]+m*dp0[j];
		}	
	}
	if(flag_Tover==1)
	{
		flag_Tover=0;
		change();
	}
}
void TIM4_IRQHandler()	 
{
    if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET) {
		flag_pwm++;
		num++;
		if(flag_pwm==10)
		{
			flag_pwm=0;
			flag_vpwm=1;
		}
	TIM_ClearITPendingBit(TIM4, TIM_IT_Update);
    }
}
void TIM3_IRQHandler()	 
{
	if(TIM_GetITStatus(TIM3,TIM_IT_Update)!= RESET)
	{
		TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
		Key();
		Moto=Position_PID(Position,Target_Position);
		Moto=Xianfu(Moto,Target_Velocity);
		Set_Pwm(Moto);
//		Led_Flash(50);
	}
}
